#include "./cmd_can.h"
#include "../xlib/gpio.h"
#include "../rtos/event_flags.h"
#include <stdio.h>

namespace pub_driver {

CmdCAN cmdCAN;

namespace cmd_can_impl {

using namespace stm32f1;
using namespace rtos::lazy;

using TX = gpio::PA<12>;
using RX = gpio::PA<11>;
using S = gpio::PA<15>;

static EventFlags eventFlags;
static constexpr uint32_t FLAG_CAN_TC = 0x01;
static constexpr uint32_t FLAG_CAN_PKG_FINISH = 0x02;

}

using namespace cmd_can_impl;

static void filter_config() {
    CAN1->FMR = CAN_FMR_FINIT;
    CAN1->FA1R = 0x00; // 过滤器先禁用

#if defined(APP_MAIN)
    CAN1->FS1R = 0x03; // 0/1 过滤器开启
#else
    CAN1->FS1R = 0x01; // 0 过滤器开启
#endif

    constexpr uint32_t MASK = ((0xFF << 8) << 3) + 0x04;

    CAN1->FM1R = 0x00; // 2个32位 标识符屏蔽
    CAN1->FFA1R = 0x00; // 关类到FIFO0

    CAN1->sFilterRegister[0].FR1 = ((conn::addr::SELF << 8) << 3) + 0x04;
    CAN1->sFilterRegister[0].FR2 = MASK;

#if defined(APP_MAIN)
    CAN1->sFilterRegister[1].FR1 = ((conn::addr::IPC << 8) << 3) + 0x04;
    CAN1->sFilterRegister[1].FR2 = MASK;
#endif

#if defined(APP_MAIN)
    CAN1->FA1R = 0x03; // 0/1 过滤器激活
#else
    CAN1->FA1R = 0x01; // 0 过滤器激活
#endif
    CAN1->FMR = 0x00;
}

void CmdCAN::init() {
    eventFlags.init();
    
#ifdef CAN_RECV_IPC
    ipcReceiver.init();
#endif

#ifdef CAN_RECV_CARGO
    cargoReceiver.init();
#endif

#ifdef CAN_RECV_HEAT
    heatReceiver.init();
#endif

#ifdef CAN_RECV_PEARL
    pearlReceiver.init();
#endif


#ifdef CAN_RECV_ICE
    iceReceiver.init();
#endif


    eventFlags.set_flags(FLAG_CAN_TC | FLAG_CAN_PKG_FINISH);

    rcc::enable<rcc::PeriphCan1, TX, RX, S>();

    gpio::config<
        gpio::Cfg<TX, gpio::Mode::OUT_50MHz_AF_PP>,
        gpio::Cfg<RX, gpio::Mode::INPUT_FLOAT>,
        gpio::Cfg<S, gpio::Mode::INPUT_FLOAT>
    >();

    CAN1->MCR = CAN_MCR_INRQ +
        CAN_MCR_ABOM +
        CAN_MCR_NART;
    while ((CAN1->MSR & CAN_MSR_INAK) == 0);

    // 36 / (3 + TBS2 + TBS1) / (1 + BRP)

    constexpr uint32_t TBS2 = 2;
    constexpr uint32_t TBS1 = 1;
    constexpr uint32_t BRP = 5 ;

    CAN1->BTR = (TBS2 << 20) +
        (TBS1 << 16) +
        BRP;

    CAN1->MCR &= ~CAN_MCR_INRQ;
    while ((CAN1->MSR & CAN_MSR_INAK) != 0x00);

    filter_config();

    CAN1->IER = CAN_IER_TMEIE + CAN_IER_FMPIE0;

    NVIC_EnableIRQ(USB_HP_CAN1_TX_IRQn);
    NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
}   

static void write_frame(uint16_t id, const uint8_t *buf8, uint8_t len) {
    eventFlags.wait_any_flags(FLAG_CAN_TC);

    CAN1->sTxMailBox[0].TDTR = len;
    
    CAN1->sTxMailBox[0].TDLR = buf8[0] | 
        (buf8[1] << 8) |
        (buf8[2] << 16) |
        (buf8[3] << 24);
    
    CAN1->sTxMailBox[0].TDHR = buf8[4] |
        (buf8[5] << 8) |
        (buf8[6] << 16) |
        (buf8[7] << 24);
    
    CAN1->sTxMailBox[0].TIR = (id << 3) + 
        CAN_TI0R_IDE + 
        CAN_TI0R_TXRQ;
}

static void recv_frame() {
    conn::CANFrame frame;
    frame.id = CAN1->sFIFOMailBox[0].RIR >> 3;
    frame.len = CAN1->sFIFOMailBox[0].RDTR & 0x0F;

    uint32_t tmp = CAN1->sFIFOMailBox[0].RDLR;
    frame.buf[0] = tmp;
    frame.buf[1] = tmp >> 8;
    frame.buf[2] = tmp >> 16;
    frame.buf[3] = tmp >> 24;
    
    tmp = CAN1->sFIFOMailBox[0].RDHR;
    frame.buf[4] = tmp;
    frame.buf[5] = tmp >> 8;
    frame.buf[6] = tmp >> 16;
    frame.buf[7] = tmp >> 24;

    CAN1->RF0R = CAN_RF0R_RFOM0;

    switch (frame.src()) {

#ifdef CAN_RECV_IPC
        case conn::addr::IPC: cmdCAN.ipcReceiver.post(frame); break;
#endif

#ifdef CAN_RECV_CARGO
        case conn::addr::CARGO: cmdCAN.cargoReceiver.post(frame); break;
#endif

#ifdef CAN_RECV_HEAT
        case conn::addr::HEAT: cmdCAN.heatReceiver.post(frame); break;
#endif

#ifdef CAN_RECV_PEARL
        case conn::addr::PEARL: cmdCAN.pearlReceiver.post(frame); break;
#endif

#ifdef CAN_RECV_ICE
        case conn::addr::ICE: cmdCAN.iceReceiver.post(frame); break;
#endif

    }
}

void CmdCAN::write_raw(const uint8_t *buf, usize_t len) {

    // uint8_t dst = buf[3];
    // uint8_t src = buf[4];
    uint16_t id = (buf[3] << 8) + buf[4];
    uint8_t index = 0;

    while (index < len) {
        uint8_t n = len - index;
        if (n > 8) {
            n = 8;
        }
        write_frame(id, buf + index, n);
        index += n;
    }
    eventFlags.set_flags(FLAG_CAN_PKG_FINISH);
}

void CmdCAN::wait_write_finish() {
    eventFlags.wait_any_flags(FLAG_CAN_PKG_FINISH);
}

void CmdCAN::write(const uint8_t *buf, usize_t len) {
    wait_write_finish();
    write_raw(buf, len);
}

void CmdCAN::log(const char *format, va_list ap) {
    using namespace conn;

    wait_write_finish();
    int n = vsprintf(
        reinterpret_cast<char *>(m_write_buf + 8),
        format,
        ap
    );
    m_write_buf[6] = conn::proto::pub::NOTIFY_LOG;
    m_write_buf[7] = n;
    usize_t len = make_proto_data<conn::type::NOTIFY>(m_write_buf, addr::IPC, 2 + n);
    write_raw(m_write_buf, len);
}

}

using namespace pub_driver;

extern "C"
void USB_LP_CAN1_RX0_IRQHandler() {
    if ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) {
        recv_frame();
    }
}

extern "C"
void USB_HP_CAN1_TX_IRQHandler() {

    if (CAN1->TSR & CAN_TSR_RQCP0) {
        CAN1->TSR = CAN_TSR_RQCP0;
        eventFlags.set_flags(FLAG_CAN_TC);
    }    
}




